//
// Created by lqk on 2022/1/29.
//

#include "LegsController.h"

void LegsController::UpdateMotorsCommand(LegCommand* const legsCommand,LegState* const legsState) {
    for(int leg=0;leg<4;leg++){
        //TODO Consider reduction gear ratio

        // * Cartesian PD controller + statics force + feedforward force + joint PD controller
        Eigen::Vector3f legTorque=legsCommand[leg].m_tauFeedforward;
        Eigen::Vector3f footForce=legsCommand[leg].m_footForce;
        footForce+=legsCommand[leg].m_kpCartesian*(legsCommand[leg].m_footPosDesired-legsState[leg].m_footPos);
        footForce+=legsCommand[leg].m_kdCartesian*(legsCommand[leg].m_footVelDesired-legsState[leg].m_footVel);
        legTorque+=legsState[leg].m_jacobian.transpose()*footForce;

        g_motorsCommand.tauAbadFF[leg]=legTorque(0);
        g_motorsCommand.tauHipFF[leg]=legTorque(1);
        g_motorsCommand.tauKneeFF[leg]=legTorque(2);

        g_motorsCommand.kdAbad[leg]=legsCommand[leg].m_kdJoints(0,0);
        g_motorsCommand.kdHip[leg]=legsCommand[leg].m_kdJoints(1,1);
        g_motorsCommand.kdKnee[leg]=legsCommand[leg].m_kdJoints(2,2);

        g_motorsCommand.kpAbad[leg]=legsCommand[leg].m_kpJoints(0,0);
        g_motorsCommand.kpHip[leg]=legsCommand[leg].m_kpJoints(1,1);
        g_motorsCommand.kpKnee[leg]=legsCommand[leg].m_kpJoints(2,2);

        g_motorsCommand.qDesAbad[leg]=legsCommand[leg].m_qDesired(0);
        g_motorsCommand.qDesHip[leg]=legsCommand[leg].m_qDesired(1);
        g_motorsCommand.qDesKnee[leg]=legsCommand[leg].m_qDesired(2);

        g_motorsCommand.qdDesAbad[leg]=legsCommand[leg].m_qdDesired(0);
        g_motorsCommand.qdDesHip[leg]=legsCommand[leg].m_qdDesired(1);
        g_motorsCommand.qdDesKnee[leg]=legsCommand[leg].m_qdDesired(2);
    }
}

void LegsController::UpdateLegsState(LegState* const legsState) {
    for(int leg=0;leg<4;leg++){
        legsState[leg].m_q<<g_motorsInfo.posAbad[leg],g_motorsInfo.posHip[leg],g_motorsInfo.posKnee[leg];
        legsState[leg].m_qd<<g_motorsInfo.velAbad[leg],g_motorsInfo.velHip[leg],g_motorsInfo.velKnee[leg];
    }
    m_quadkinematics.CalculateJacobians(legsState);
    m_quadkinematics.CalculateFootsPosition(legsState);
    for(int leg=0;leg<4;leg++){
        legsState[leg].m_footVel=legsState[leg].m_jacobian*legsState[leg].m_qd;
    }
}